#! /usr/bin/env python
# -*- coding: utf-8 -*-
from naoqi import ALProxy
import sys
import time
from naoqi import *
import math
import almath
import time
import argparse
import re
import thread


def run(m, x, y, t, s=[["Frequency", 1.0]]):
    m.move(x, 0, t)#["MaxStepX",0.04] ["Frequency", 1.0],["LeftStepHeight", 0.010],["RightStepHeight", 0.010],

def main(robotIP):
    PORT = 9559

    try:
        memoryProxy = ALProxy("ALMemory", robotIP, PORT)
    except Exception, e:
        print "Could not create proxy to ALMemory"
        print "Error was: ", e

    try:
        sonarProxy = ALProxy("ALSonar", robotIP, PORT)
    except Exception, e:
        print "Could not create proxy to ALSonar"
        print "Error was: "

    count = 03
    y = 0.25        # 左右移动  y = 0.55
    x = 2   # 前进#lll
    theta = -0.011
    motionProxy = ALProxy("ALMotion", robotIP, PORT)
    postureProxy = ALProxy("ALRobotPosture", robotIP, PORT)

    motionProxy.wakeUp()
    postureProxy.goToPosture("StandInit", 0.5)
    time.sleep(3)#试试

    sonarProxy.subscribe("myApplication")
   # motionProxy.setMoveArmsEnabled(True,True)#gangjiade


    while 1:
        # names = ["RAnklePitch"]
        # angles = [0.0]
        # fractionMaxSpeed = 0.2
        # motionProxy.setAngles(names, angles, fractionMaxSpeed)
        print "Left  : "
        left = memoryProxy.getData("Device/SubDeviceList/US/Left/Sensor/Value")
        print left
        print "Right  : "
        Right = memoryProxy.getData("Device/SubDeviceList/US/Right/Sensor/Value")
        print  Right
        # motionProxy.moveTo(0, -y, 0)
        if (left >= 0.4 and Right >= 0.4):
            print "continue\n\n\n"
            thread.start_new_thread(run, (motionProxy, x, 0, theta,))
            # motionProxy.move(x, 0, theta, [["Frequency", 1.0], ["LeftStepHeight", 0.1], ["RightStepHeight", 0.1],
            #                      ["RightMaxStepX", 0.1], ["LeftMaxStepX", 0.1]])
            # motionProxy.moveTo(x, 0, theta)
        # elif left<Right:

        else:
            if (count % 2 == 0):
                print "turn right\n\n\n"
            # motionProxy.moveTo(0, 0, -math.pi/2.0)
            #     motionProxy.moveTo(0, -y, 0)
                motionProxy.move(0,-0.1,0)
                while(memoryProxy.getData("Device/SubDeviceList/US/Right/Sensor/Value")<0.5 or memoryProxy.getData("Device/SubDeviceList/US/Left/Sensor/Value")<0.5):
                    time.sleep(0.1)



            # motionProxy.moveTo(0, 0, math.pi / 2.0)
        # elif (left>Right):
            elif count % 2 == 1:
                print "turn left\n\n\n"
            # motionProxy.moveTo(0, 0, math.pi/2.0)
            #     motionProxy.moveTo(0, y, 0)
                motionProxy.move(0, 0.1, 0)
                while (memoryProxy.getData("Device/SubDeviceList/US/Right/Sensor/Value")<0.5 or memoryProxy.getData("Device/SubDeviceList/US/Left/Sensor/Value")<0.5):
                    time.sleep(0.1)
            # motionProxy.move0To(0, 0, -math.pi / 2.0)
            count += 1

            # if(count==4):
            #     motionProxy.moveTo(0.8,0,0)
            #     motionProxy.move(0, -0.1, 0)
            #     while (memoryProxy.getData("Device/SubDeviceList/US/Right/Sensor/Value") < 0.5 or memoryProxy.getData("Device/SubDeviceList/US/Left/Sensor/Value") < 0.5):
            #         time.sleep(0.1)
            if (count == 4):
                motionProxy.move(2, 0, 0)
                time.sleep(3)
                motionProxy.move(0, 0, math.pi /-30)
                time.sleep(3)
                motionProxy.move(2, 0, 0)
                time.sleep(4)
            if(count==5):
                motionProxy.move(2, 0, 0)
                time.sleep(5)
                motionProxy.move(0, -0.1, 0)
                time.sleep(4)
                motionProxy.move(0, 0, math.pi / 35)
                motionProxy.move(2, 0, 0)
                time.sleep(3)
            if(count == 6):
               motionProxy.move(2, 0, 0)
               time.sleep(3)
               motionProxy.move(0, 0, math.pi/-20)
               time.sleep(3)
               motionProxy.move(2, 0, 0)
               time.sleep(3)
               # motionProxy.move(0, 0, math.pi/16)
               # time.sleep(5)
            # motionProxy.moveTo(x, 0, theta/2)
            # theta = theta - 0.0005
        time.sleep(0.1)

    sonarProxy.unsubscribe("myApplication")

    #放的时候往左斜


if __name__ == "__main__":
    robotIp = "192.168.1.14"

    if len(sys.argv) <= 1:
        print "Usage python sensors_sonar.py robotIP (optional default: 127.0.0.1)"
    else:
        robotIp = sys.argv[1]

    main(robotIp)
